/*
 * kdtree.cpp
 *
 *  Created on: Apr 27, 2011
 *      Author: tqlong
 */

#include "kdtree.h"
#include <QtCore/QVector>
#include <QtCore/QtAlgorithms>

BoundingBox::BoundingBox(int dim) :
  m_min(dim), m_max(dim), n_dim(dim)
{
  m_min.fill(POSITIVE_INFINITY);
  m_max.fill(NEGATIVE_INFINITY);
}

void
BoundingBox::accumulate(const vec& v)
{
  assert(v.n_elem == n_dim);
  for (int i = 0; i < n_dim; i++)
    {
      if (m_min[i] > v[i])
        m_min[i] = v[i];
      if (m_max[i] < v[i])
        m_max[i] = v[i];
    }
}

void
BoundingBox::accumulate(const BoundingBox& b)
{
  assert(b.n_dim == n_dim);
  for (int i = 0; i < n_dim; i++)
    {
      if (m_min[i] > b.m_min[i])
        m_min[i] = b.m_min[i];
      if (m_max[i] < b.m_max[i])
        m_max[i] = b.m_max[i];
    }
}

const vec&
BoundingBox::min() const
{
  return m_min;
}
const vec&
BoundingBox::max() const
{
  return m_max;
}

double
BoundingBox::lowerBound(const vec& v) const
{
  double s = 0;
  for (int i = 0; i < n_dim; i++)
    {
      if (v[i] < m_min[i])
        s += (v[i] - m_min[i]) * (v[i] - m_min[i]);
      if (v[i] > m_max[i])
        s += (v[i] - m_max[i]) * (v[i] - m_max[i]);
    }
  return sqrt(s);
}

void
BoundingBox::print() const
{
  std::cerr << "min=\n" << m_min << "max=\n" << m_max << std::endl;
}

KDTree::KDTree(const dataset_type& data) :
  __Base(data), m_box(n_dim())
{
}

KDTree::KDTree(KDTree* parent, int s, int e) :
  __Base(parent, s, e), m_box(n_dim())
{
}

const BoundingBox&
KDTree::box() const
{
  return m_box;
}

BoundingBox&
KDTree::stats()
{
  return m_box;
}

const BoundingBox&
KDTree::stats() const
{
  return m_box;
}

void
KDTree::split()
{
  //    Log::m_log << "split ms=" << m_s << " me=" << m_e << endl;
  //    for_all_node_data(i)
  //      Log::m_log << m_idx[m_s+i] << " ";
  //    Log::m_log << endl;
  if (n() <= 10)
    return;
  double variance = 0, split_value = 0;
  int dim = 0;
  variance = find_max_variance(dim);
  //    Log::m_log << "dim=" << dim << " var=" << variance << endl;
  if (variance < 1e-10 || dim < 0)
    return;
  split_value = find_split_value(dim);
  //    Log::m_log << "val=" << split_value << endl;
  split(dim, split_value);
}

void
KDTree::findKNN(const vec& query, std::vector<int>& knnIdx,
    std::vector<double>& minBound)
{
  if (m_box.lowerBound(query) >= minBound.back())
    return;
  if (n_child() == 0)
    Tree::findKNN(query, knnIdx, minBound);
  else
    {
      for_all_node_child(i)
        {
          child(i)->findKNN(query, knnIdx, minBound);
        }
    }
}

double
KDTree::find_max_variance(int& dim_max) const
{
  double max_variance = 0;
  dim_max = -1;
  for (int dim = 0; dim < n_dim(); dim++)
    {
      double min = min_dim(dim), max = max_dim(dim), variance = max - min;
      if (max_variance < variance)
        {
          dim_max = dim;
          max_variance = variance;
        }
    }
  return max_variance;
}

double
KDTree::find_split_value(int dim) const
{
  QVector<double> x(n());
  for_all_node_data(i)
    x[i] = get(i)[dim];
  qSort(x.begin(), x.end());
  double split_value = x[n() / 2];
  double max_gap = 0.0;
  if (n() / 2 - 1 >= 0)
    {
      split_value = (x[n() / 2] + x[n() / 2 - 1]) / 2;
      max_gap = x[n() / 2] - x[n() / 2 - 1];
    }

  for (int i = n() / 4; i < 3 * n() / 4 - 1; i++)
    {
      double gap = x[i + 1] - x[i];
      if (gap > max_gap)
        {
          max_gap = gap;
          split_value = (x[i + 1] + x[i]) / 2;
        }
    }
  return split_value;
}

double
KDTree::min_dim(int dim) const
{
  double min = POSITIVE_INFINITY;
  for_all_node_data(i)
    {
      double v = get(i)[dim];
      if (v < min)
        min = v;
    }
  return min;
}

double
KDTree::max_dim(int dim) const
{
  double max = NEGATIVE_INFINITY;
  for_all_node_data(i)
    {
      double v = get(i)[dim];
      if (v > max)
        max = v;
    }
  return max;
}

void
KDTree::split(int dim, double split_value)
{
  int n_left = 0;
  for_all_node_data(i)
    {
      double v = get(i)[dim];
      //      Log::m_log << v << " ";
      if (v <= split_value)
        {
          swap(n_left, i);
          n_left++;
        }
    }
  //    Log::m_log << endl;
  //    Log::m_log << "n_left="<< n_left << endl;
  if (n_left > 0 && n_left < n())
    {
      //      Log::m_log << "create left child ms="<< m_s << " me=" << n_left+m_s << endl;
      //      Log::m_log << "create right child ms="<< m_s+n_left << " me=" << n()+m_s << endl;
//      Tree* left = new KDTree(this, m_s, n_left + m_s);
//      Tree* right = new KDTree(this, n_left + m_s, n() + m_s);
      Tree* left = this->createChild(m_s, n_left + m_s);
      Tree* right = this->createChild(n_left + m_s, n() + m_s);
      addChild(left);
      addChild(right);
    }
}
